/**
  ****************************(C) COPYRIGHT 2023 Polarbear*************************
  * @file       rm_serial_driver.cpp
  * @brief      串口通信模块
  * @note       感谢@ChenJun创建本模块并开源，
  *             现内容为北极熊基于开源模块进行修改并适配自己的车车后的结果。
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2022            ChenJun         1. done
  *  V1.0.1     2023-12-11      Penguin         1. 添加与rm_rune_dector_node模块连接的Client
  *  V1.0.2     2024-3-1        LihanChen       1. 添加导航数据包，并重命名packet和相关函数
  *
  @verbatim
  =================================================================================

  =================================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2023 Polarbear*************************
  */

#ifndef RM_SERIAL_DRIVER__RM_SERIAL_DRIVER_HPP_
#define RM_SERIAL_DRIVER__RM_SERIAL_DRIVER_HPP_

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/Float64.h>
#include <std_srvs/Trigger.h>
#include <dynamic_reconfigure/Reconfigure.h>

#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/buffer.h>

#include <robots_msgs/Nav.h>

#include <functional>
#include <memory>
#include <string>
#include <vector>

#include "serial_driver/JLinkInterface.hpp"
#include "serial_driver/crc.hpp"
#include "serial_driver/packet.hpp"
namespace rm_serial_driver
{
// define flowcontrol mode
enum class FlowControl {
  NONE,
  HARDWARE,
  SOFTWARE
};
// define parity check mode
enum class Parity {
  NONE,
  ODD,
  EVEN
};
// define stopbit mode
enum StopBits {
  ONE,
  ONE_POINT_FIVE,
  TWO
};

class RMSerialDriver
{
public:
// init
  RMSerialDriver(const ros::NodeHandle& nh);
// destroy
  ~RMSerialDriver();

private:
//
  void getParams();

  void receiveDataNav(const ros::TimerEvent&);

  void sendDataNav(const geometry_msgs::TwistConstPtr&);

  void reopenPort();

  void setParam(const std::string& param_name, int value);

  void initJBlink();

  void safetyTrigger(const ros::TimerEvent&);

  void odomCB(const nav_msgs::OdometryConstPtr& odomPtr){
    odom = *odomPtr;
  }

  ros::NodeHandle nh_;
  
  // Serial port
  std::string device_name_;
  std::unique_ptr<serial::Serial> serial_driver_;
  std::unique_ptr<JLinkOperations> JLink_Operator_;

  //tf2 listener
  std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  ros::Time last_msg_time_;

  // Param client to set detect_colr
  bool initial_set_param_ = false;
  uint8_t previous_receive_color_ = 0;
  ros::ServiceClient detector_param_client_;

  // ROS communicators
  ros::Publisher nav_pub;
  ros::Subscriber cmd_vel_sub_;
  ros::Subscriber odom_sub;

  // ROS messages
  nav_msgs::Odometry odom;
  geometry_msgs::Twist cmd_vel;

  // For debug usage
  ros::Publisher latency_pub_;
  ros::Publisher marker_pub_;
  ros::Publisher trackor_debug_pub_;

  ros::Timer receive_timer_;
  ros::Timer safety_timer_;

  // For debug usage
  int bytes_read;
  int bytes_write;

  int stop_jb_read_write;
};
}  // namespace rm_serial_driver

#endif  // RM_SERIAL_DRIVER__RM_SERIAL_DRIVER_HPP